#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
sys.path.append("..")
sys.path.append("../../")
from lib.BwRobotLib import BwRobotLib
from lib.MiniBotControlLib import MiniBotControlLib
from RobotSnake.SnakeRobot import SnakeRobot
import keyboard
import time
import _thread


robotlib = BwRobotLib()
snake = SnakeRobot(robotlib)
#robotlib.mclib.SetIsDebug(True)

#连接机器人设备
robotlib.connectRobot("192.168.3.46",55001)
#robotlib.connectRobot("10.10.100.254",55000)

#加载配置（兼容VS项目启动方式及文件直接启动方式）
import os,inspect
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"
robotlib.loadConfig(cfile)

#配置操作臂
snake.bindIds(robotlib.module_ids)

#-----------------------------------------------------------------------------
#仿生蛇
#-----------------------------------------------------------------------------


#运行线程
def snakeRun(threadName,arg):
    snake.run()

input("输入任意字符 Enter 开始初始化...")
snake.stop()
_thread.start_new_thread(snakeRun,("runthread",snake))


#运行
def runDemo():
  waitSeconds = 2
  snake.goForward()
  time.sleep(waitSeconds)
  snake.goBack()
  time.sleep(waitSeconds)
  snake.turnLeft()
  time.sleep(1)
  snake.turnRight()
  time.sleep(1)
  snake.stop()

while(1):
  input("输入任意字符 Enter 运行演示示例...")
  runDemo()

